/*
* Copyright (c) 2017, <copyright holder> <email>
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*     * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright
*     notice, this list of conditions and the following disclaimer in the
*     documentation and/or other materials provided with the distribution.
*     * Neither the name of the <organization> nor the
*     names of its contributors may be used to endorse or promote products
*     derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY <copyright holder> <email> ''AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <copyright holder> <email> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/

#include <tuw_voronoi_graph/segment_expander.h>
#include <ros/ros.h> //DEBUG

namespace tuw_graph
{
Segment_Expander::Segment_Expander() {}

void Segment_Expander::Initialize(cv::Mat &_map, cv::Mat &_distField, cv::Mat &_voronoiPath)
{
    nx_ = _map.cols;
    ny_ = _map.rows;
    ns_ = nx_ * ny_;
    distance_field_.reset(new float[nx_ * ny_]);
    voronoi_graph_.reset(new int8_t[nx_ * ny_]);
    global_map_.reset(new int8_t[nx_ * ny_]);
    float *distance_field = distance_field_.get();
    int8_t *voronoi_graph = voronoi_graph_.get();
    int8_t *global_map = global_map_.get();
    for (int i = 0; i < ns_; i++)
    {
        distance_field[i] = ((float *) _distField.data)[i];
        voronoi_graph[i] = ((int8_t *) _voronoiPath.data)[i];
        global_map[i] = ((int8_t *) _map.data)[i];
    }
}

void Segment_Expander::optimizeSegments(std::vector<Segment> &_segments, float _maxPixelsCrossing,
                                        float _maxPixelsEndSegment)
{
    // Move all segments with distance less than _maxPixelCrossing into the crossing
    for (uint32_t i = 0; i < _segments.size(); i++)
    {
        if (!_segments[i].getOptStart())
        {
            optimizeSegmentsAroundPoint(_segments, _segments[i].getStart(), _maxPixelsCrossing, i);
        }
        if (!_segments[i].getOptEnd())
        {
            optimizeSegmentsAroundPoint(_segments, _segments[i].getEnd(), _maxPixelsCrossing, i);
        }
    }
    // Remove segments with zero length...
    for (uint32_t i = 0; i < _segments.size(); i++)
    {
        if ((_segments[i].getStart() - _segments[i].getEnd()).norm() == 0)
        {
            removeSegmentFromList(i, _segments);
            // if we erase one segment we have to check the same spot again...
            i--;
        }
    }
    // Important do this only in the end of the optimization
    for (uint32_t i = 0; i < _segments.size(); i++)
    {
        //Its important to have a sorted segment list by id... Therefore we need to update all segments when we remove one of them
        //As well we need to remove all existing neighbors before erasing...
        if ((_segments[i].getStart() - _segments[i].getEnd()).norm() < _maxPixelsEndSegment &&
            (_segments[i].getPredecessors().size() == 0 || _segments[i].getSuccessors().size() == 0))
        {
            removeSegmentFromList(i, _segments);
            i--;
        }
    }
}

void Segment_Expander::removeSegmentFromList(const uint32_t _id, std::vector<Segment> &_segments)
{
    //Remove neighbors
    for (uint32_t pd : _segments[_id].getPredecessors())
    {
        _segments[pd].cleanNeighbors(_id);
    }
    for (uint32_t sc : _segments[_id].getSuccessors())
    {
        _segments[sc].cleanNeighbors(_id);
    }
    _segments.erase(_segments.begin() + _id);
    //Reorder graph
    for (auto & _segment : _segments)
    {
        _segment.decreaseNeighborIdAbove(_id);
    }
}

void Segment_Expander::optimizeSegmentsAroundPoint(std::vector<Segment> &_segments, const Eigen::Vector2d &pt,
                                                   float maxPixels, int startIndex)
{
    //References to segments of a crossing
    std::vector<Segment *> connectedSegmnetsStart;
    std::vector<Segment *> connectedSegmnetsEnd;
    //find all segments to optimize
    for (uint32_t i = startIndex; i < _segments.size(); i++)
    {
        if (!_segments[i].getOptStart())
        {
            if ((_segments[i].getStart() - pt).norm() < maxPixels)
            {
                connectedSegmnetsStart.push_back(&_segments[i]);
                _segments[i].setStart(pt);
                _segments[i].getOptStart() = true;
            }
        }
        if (!_segments[i].getOptEnd())
        {
            if ((_segments[i].getEnd() - pt).norm() < maxPixels)
            {
                connectedSegmnetsEnd.push_back(&_segments[i]);
                _segments[i].setEnd(pt);
                _segments[i].getOptEnd() = true;
            }
        }
    }
    //Connect all optimized segments
    for (uint32_t i = 0; i < connectedSegmnetsStart.size(); i++)
    {
        for (uint32_t j = 0; j < connectedSegmnetsStart.size(); j++)
        {
            if (i != j)
            {
                if (!(*connectedSegmnetsStart[i]).containsPredecessor((*connectedSegmnetsStart[j]).getId()))
                {
                    (*connectedSegmnetsStart[i]).addPredecessor((*connectedSegmnetsStart[j]).getId());
                }
            }
        }
        for (auto & j : connectedSegmnetsEnd)
        {
            if (!(*connectedSegmnetsStart[i]).containsPredecessor((*j).getId()))
            {
                (*connectedSegmnetsStart[i]).addPredecessor((*j).getId());
            }
        }
    }
    for (uint32_t i = 0; i < connectedSegmnetsEnd.size(); i++)
    {
        for (auto & j : connectedSegmnetsStart)
        {
            if (!(*connectedSegmnetsEnd[i]).containsSuccessor((*j).getId()))
            {
                (*connectedSegmnetsEnd[i]).addSuccessor((*j).getId());
            }
        }
        for (uint32_t j = 0; j < connectedSegmnetsEnd.size(); j++)
        {
            if (i != j)
            {
                if (!(*connectedSegmnetsEnd[i]).containsSuccessor((*connectedSegmnetsEnd[j]).getId()))
                {
                    (*connectedSegmnetsEnd[i]).addSuccessor((*connectedSegmnetsEnd[j]).getId());
                }
            }
        }
    }
}

const std::vector<tuw_graph::Segment> Segment_Expander::splitPath(
        const std::vector<Eigen::Vector2d> &_path, const float _minimum_length)
{
    std::vector<tuw_graph::Segment> segments;
    if (_path.size() > 1 && _path.size() <= _minimum_length)
    {
        float min_seg_width = getMinSegmentWidth(_path);
        Segment seg(_path, 2 * min_seg_width);
        segments.push_back(seg);
    }
    else if (_path.size() > 1 && _path.size() > _minimum_length)
    {
        uint32_t nr_segments = _path.size() / _minimum_length;
        float real_length = std::ceil((float) _path.size() / (float) nr_segments);
        for (uint32_t i = 0; i < nr_segments; i++)
        {
            //Segment
            std::vector<Eigen::Vector2d> map_path_segment;
            for (uint32_t j = std::max<int>(0, i * real_length);
                 j <= (i + 1) * real_length && j < _path.size(); j++)
            {
                map_path_segment.emplace_back(_path[j][0], _path[j][1]);
            }
            float min_seg_width = getMinSegmentWidth(map_path_segment);
            Segment newSegment(std::vector<Eigen::Vector2d>(map_path_segment), 2 * min_seg_width);
            //Push back and add neighbors
            segments.push_back(newSegment);
            if (i > 0)
            {
                segments[segments.size() - 2].addSuccessor(segments[segments.size() - 1].getId());
                segments[segments.size() - 1].addPredecessor(segments[segments.size() - 2].getId());
            }
        }
    }
    return segments;
}

std::vector<Segment> Segment_Expander::getGraph(const std::vector<std::vector<Eigen::Vector2d>> &_endPoints,
        float *_potential, const float _min_length, float _optimizePixelsCrossing, const float _optimizePixelsEndSegments)
{
    Segment::resetId();
    // We need at least a 10th of max length for seg optimization to find unconnected edges from skeletonizing
    // 我们需要至少十分之一的最大长度来进行分段优化，以便从骨架化中找到未连接的边
    _optimizePixelsCrossing = std::max<float>(_min_length / 10, _optimizePixelsCrossing);
    // To be able to change the segments in the crossing class it has to be somehow accessable over the heap -> shared_ptr
    // 为了能够更改交叉类中的段，它必须在堆上以某种方式可访问->shared_ptr
    std::shared_ptr<std::vector<Segment >> segments = std::make_shared<std::vector<Segment>>();
    std::vector<std::vector<Eigen::Vector2d>> endPoints = _endPoints;
    // Convert endpoints to crossings 将端点转换为交叉点
    std::vector<tuw_graph::Crossing> crossings_;
    for (uint32_t i = 0; i < endPoints.size(); i++)
    {
        tuw_graph::Crossing c(std::vector<Eigen::Vector2d> (endPoints[i]));
        c.setSegmentReference(segments);
        crossings_.push_back(c);
    }
    // Find all paths connecting the crossings 查找连接十字路口的所有路径
    while (!endPoints.empty())
    {
        std::vector<Eigen::Vector2d> startCrossing = endPoints.back();
        if (!startCrossing.empty())
        {
            Eigen::Vector2d startPoint = startCrossing.back();
            // Reset the potential of each crossing endpoint to prepare for the expansion
            // 重置每个交叉点的潜力，为扩建做准备
            for (const Eigen::Vector2d &cVec : startCrossing)
            {
                Index idx(cVec[0], cVec[1], nx_, 0, 0, 0);
                _potential[idx.i] = 0;
            }
            Index start(startPoint[0], startPoint[1], nx_, 0, 0, 0);
            std::vector<Eigen::Vector2d> path = getPath(start, _potential, endPoints);
            std::vector<Segment> pathSegments = splitPath(path, _min_length);
            segments->insert(segments->end(), pathSegments.begin(), pathSegments.end());
            removeEndpoint(start, endPoints);
        }
        else endPoints.pop_back();
    }
    // Find the correct crossing for each segment 为每个分段找到正确的交叉点
    for (Segment &s : (*segments))
    {
        for (Crossing &c : crossings_)
        {
            c.tryAddSegment(s);
        }
    }
    // Optimize Crossings by moving the center and removing unecessary "double crossings"
    // 通过移动中心和删除不必要的“双交叉口”来优化交叉口
    optimizeSegments((*segments), _optimizePixelsCrossing, _optimizePixelsEndSegments);
    return (*segments);
}

float Segment_Expander::getMinSegmentWidth(const std::vector<Eigen::Vector2d> &_path)
{
    if (_path.empty()) return 0;
    Index p1(_path[0][0], _path[0][1], nx_, 0, 0, 0);
    float minimum_path_space = distance_field_[p1.i];
    for (const Eigen::Vector2d &point : _path)//auto it = _path.begin(); it != _path.end(); ++it)
    {
        Index p(point[0], point[1], nx_, 0, 0, 0);
        minimum_path_space = std::min<float>(distance_field_[p.i], minimum_path_space);
    }
    return minimum_path_space;
}

std::vector<std::vector<Eigen::Vector2d> > Segment_Expander::calcEndpoints(float *_potential)
{
    std::vector<std::vector<Eigen::Vector2d> > endpoints;
    std::fill(_potential, _potential + ns_, -1);
    for (int i = 0; i < ns_; i++)
    {
        //dont examine allready found potentials
        if (_potential[i] >= 0) continue;
        //only look for voronoi points
        if (voronoi_graph_[i] >= 0) continue;
        if (nrOfNeighbours(i) > 2)
        {
            std::vector<Eigen::Vector2d> crossing = expandCrossing(Index(i, 0, 0, 0), _potential);
            endpoints.push_back(crossing);
        }
    }
    return std::vector<std::vector<Eigen::Vector2d> >(endpoints);
}

bool Segment_Expander::checkSegmentPoint(const Segment_Expander::Index &_point)
{
    //Check Boundries
    if (_point.i < 0 || _point.i > ns_) return false;
    if (global_map_[_point.i] > 0) return false;
    if (voronoi_graph_[_point.i] >= 0) return false;
    return nrOfNeighbours(_point.i) == 2;
}

std::vector<Eigen::Vector2d> Segment_Expander::getPath(const Segment_Expander::Index &_start, float *_potential,
                                                       const std::vector<std::vector<Eigen::Vector2d>> &_endpoints)
{
    std::vector<Eigen::Vector2d> points;
    int cycle = 0;
    int cycles = ns_;
    _potential[_start.i] = _start.potential;
    Index current(_start.i, _potential[_start.i], 0, _potential[_start.i]);
    //min_d = distance_field_[_start.i];
    //clear the queue
    clearpq(queue_);
    queue_.push(current);
    while (!(queue_.empty()) && (cycle < cycles))
    {
        if (queue_.empty()) break;
        current = queue_.top();
        queue_.pop();
        //min_d = std::min<float>(min_d, distance_field_[current.i]);
        points.emplace_back(current.getX(nx_), current.getY(nx_));
        if (current.i != _start.i && isEndpoint(current, _endpoints))
        {
            Eigen::Vector2d point;
            point[0] = current.getX(nx_);
            point[1] = current.getY(nx_);
            return points;
        }
        else
        {
            addExpansionCandidate(current, current.offsetDist(1, 0, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(0, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, 0, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(0, -1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(1, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, -1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(1, -1, nx_, ny_), _potential);
        }
        cycle++;
    }
    float dx = points.front()[0] - points.back()[0];
    float dy = points.front()[1] - points.back()[1];
    if (!findEdgeSegments_ || points.size() <= 3 || std::sqrt(dx * dx + dy * dy) <= 3) points.clear();
    return points;
}

Eigen::Vector2d Segment_Expander::expandSegment(Segment_Expander::Index _start, float *_potential,
                                                const std::vector<std::vector<Eigen::Vector2d>> &_endpoints)
{
    std::vector<Eigen::Vector2d> points;
    int cycle = 0;
    int cycles = ns_;
    _potential[_start.i] = _start.potential;
    Index current(_start.i, _potential[_start.i], 0, _potential[_start.i]);
    //clear the queue
    clearpq(queue_);
    queue_.push(current);
    while (!(queue_.empty()) && (cycle < cycles))
    {
        if (queue_.empty()) break;
        current = queue_.top();
        queue_.pop();
        if (current.i != _start.i && isEndpoint(current, _endpoints))
        {
            Eigen::Vector2d point;
            point[0] = current.getX(nx_);
            point[1] = current.getY(nx_);
            return point;
        }
        else
        {
            addExpansionCandidate(current, current.offsetDist(1, 0, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(0, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, 0, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(0, -1, nx_, ny_), _potential);

            addExpansionCandidate(current, current.offsetDist(1, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, -1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(1, -1, nx_, ny_), _potential);
        }
        cycle++;
    }
    if (findEdgeSegments_)
        return Eigen::Vector2d(current.getX(nx_), current.getY(nx_));
    else return Eigen::Vector2d(_start.getX(nx_), _start.getY(nx_));
}

std::vector<Eigen::Vector2d> Segment_Expander::expandCrossing(const Index &_start, float *_potential)
{
    std::vector<Eigen::Vector2d> points;
    int cycle = 0;
    int cycles = ns_;
    _potential[_start.i] = _start.potential;
    Index current(_start.i, _potential[_start.i], 0, _potential[_start.i]);
    // clear the queue
    clearpq(queue_);
    queue_.push(current);
    while (!(queue_.empty()) && (cycle < cycles))
    {
        if (queue_.empty()) break;
        current = queue_.top();
        queue_.pop();
        if (nrOfNeighbours(current.i) <= 2)
        {
            Eigen::Vector2d point;
            point[0] = current.getX(nx_);
            point[1] = current.getY(nx_);
            points.push_back(point);
            // Reset potential to find point
            _potential[current.i] = -2;
        }
        else
        {
            addExpansionCandidate(current, current.offsetDist(1, 0, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(0, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, 0, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(0, -1, nx_, ny_), _potential);

            addExpansionCandidate(current, current.offsetDist(1, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, 1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(-1, -1, nx_, ny_), _potential);
            addExpansionCandidate(current, current.offsetDist(1, -1, nx_, ny_), _potential);
        }
        cycle++;
    }
    return points;
}

void Segment_Expander::addExpansionCandidate(const Index &current, const Index &next, float *potential)
{
    float potentialPrev = potential[current.i];
    // Check Boundries
    if (next.i < 0 || next.i > ns_) return;
    // Dont update allready found potentials
    if (potential[next.i] >= 0) return;
    if (global_map_[next.i] > 0) return;
    if (voronoi_graph_[next.i] >= 0) return;
    float pot = potentialPrev + 1;
    float dist = 0;
    float weight = pot;
    potential[next.i] = pot;
    queue_.push(Index(next.i, weight, dist, pot));
}


uint32_t Segment_Expander::nrOfNeighbours(uint32_t i) const
{
    // 0 1 2
    // 7   3
    // 6 5 4
    uint32_t neighbours[8] =
    {
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(-1, 1, nx_, ny_)).i] < 0),
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(0, 1, nx_, ny_)).i] < 0),
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(1, 1, nx_, ny_)).i] < 0),
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(1, 0, nx_, ny_)).i] < 0),
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(1, -1, nx_, ny_)).i] < 0),
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(0, -1, nx_, ny_)).i] < 0),
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(-1, -1, nx_, ny_)).i] < 0),
            (voronoi_graph_[(Index(i, 0, 0, 0).offsetDist(-1, 0, nx_, ny_)).i] < 0)
    };
    // Remove double neighbours e.g.:
    //  x x 0    0 1 2
    //  0 x 0    7   3
    //  0 x x    6 5 4
    // should result in 2
    uint32_t num_neighbours =
            neighbours[0] + neighbours[1] + neighbours[2] + neighbours[3] +
            neighbours[4] + neighbours[5] + neighbours[6] + neighbours[7];
    if (neighbours[0] && neighbours[1] && !neighbours[2]) num_neighbours--;
    if (!neighbours[0] && neighbours[1] && neighbours[2]) num_neighbours--;
    if (neighbours[2] && neighbours[3] && !neighbours[4]) num_neighbours--;
    if (!neighbours[2] && neighbours[3] && neighbours[4]) num_neighbours--;
    if (neighbours[4] && neighbours[5] && !neighbours[6]) num_neighbours--;
    if (!neighbours[4] && neighbours[5] && neighbours[6]) num_neighbours--;
    if (neighbours[6] && neighbours[7] && !neighbours[0]) num_neighbours--;
    if (!neighbours[6] && neighbours[7] && neighbours[0]) num_neighbours--;
    return num_neighbours;
}

void Segment_Expander::removeEndpoint(const Segment_Expander::Index &_current,
                                      std::vector<std::vector<Eigen::Vector2d> > &_endpoints)
{
    for (uint32_t j = 0; j < _endpoints.size(); j++)
    {
        if (!_endpoints[j].empty())
        {
            for (uint32_t i = 0; i < _endpoints[j].size(); i++)
            {
                if ((int) (_endpoints[j][i][0]) == _current.getX(nx_) &&
                    (int) (_endpoints[j][i][1]) == _current.getY(nx_))
                {
                    _endpoints[j].erase(_endpoints[j].begin() + i);
                    if (_endpoints[j].empty())
                    {
                        _endpoints.erase(_endpoints.begin() + j);
                    }
                }
            }
        }
        else _endpoints.erase(_endpoints.begin() + j);
    }
}

bool Segment_Expander::isEndpoint(Segment_Expander::Index &_current,
                                  const std::vector<std::vector<Eigen::Vector2d>> &_endpoints)
{
    for (auto it_crossing = _endpoints.begin(); it_crossing != _endpoints.end(); it_crossing++)
    {
        for (auto it = it_crossing->begin(); it != it_crossing->end(); it++)
        {
            if ((*it)[0] == _current.getX(nx_) && (*it)[1] == _current.getY(nx_)) return true;
        }
    }
    return false;
}
}
